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A NUMERICAL ALGORITHM FOR SINGULAR OPTIMAL LQ 

CONTROL SYSTEMS 

MARINA DELGADO-TELLEZ AND ALBERTO IBORT 



Abstract. A numerical algorithm to obtain the consistent conditions satisfied by 
singular arcs for singular linear-quadratic optimal control problems is presented. 
The algorithm is based on the presymplectic constraint algorithm (PCA) by Gotay- 
Nester [11[ 126] that allows to solve presymplectic hamiltonian systems and that 
provides a geometrical framework to the Dirac-Bergmann theory of constraints for 
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-. 1 singular Lagrangian systems [9] . The numerical implementation of the algorithm 

is based on the singular value decomposition that, on each step allows to construct 
a semi-explicit system. Several examples and experiments are discussed, among 
them a family of arbitrary large singular LQ systems with index 3 and a family of 
examples of arbitrary large index, all of them exhibiting stable behaviour. 
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1. Introduction 

Singular problems in optimal control problems and in the calculus of variations 
have been widely considered from different perspectives. A singular perturbation 
approach has been often the preferred approach to singular optimal control theory 
(see for instance |15[ [16[ [22] and references therein). More recently Jurdjevic has 
proposed a different viewpoint by introducing Lie-theoretic methods in the problem 
[14] . A similar problem in the calculus of variations has had a very different history 
mainly due to the physical insight derived from its appearance in mechanics and 
field theory, (see for instance Carihena (3|). P. A.M. Dirac took a brilliant approach 
introducing a recursive analysis to extract the integrable or solvable part of the system 
[9|. Dirac's approach consists essentially on a recursive consistency scheme where 
the original system of implicit Euler-lagrange equations obtained from the extremal 
conditions are restricted to a smaller and smaller subset on state space by imposing 
the existence of at least one solution to the problem passing through them. Such 
approach has been adapted to the problem of optimal control by Volkaert [?] , Lopez 
and Martinez [19] . Guerra [13] and Delgado and Ibort [6j by transforming it into a 
descriptor system. A geometric version of Dirac's constraint algorithm was presented 
by Gotay and Nester [11] for presymplectic systems. This algorithm was extended 
and generalized later on to more general situations (see for instance [?]) arriving to 
essentially the same geometrical algorithm devised by Rabier and Rehinbold [24] to 
deal with Differential- Algebraic equations (DAE's). A variety of ideas and techniques 
have been developed along the years to analyze DAE's (see for instance [2], [23] or 
the recent book |18J) and recently various of these approaches have been applied 
to singular optimal control problems (see [17], pQ for instance). However the PCA 
algorithm was proposed for the first time in control theory by [26], and Lopez and 
Martinez [19J, Cortes, de Leon, Martin de Diego and Martinez [5] and it has been 
discussed by Delgado and Ibort [H [7J [SJ in the specific context of singular control 
theory. Another form of this algorithm was applied to the LQ singular case by Guerra 

In this paper we will concentrate on singular linear-quadratic systems. We will 
transform such algorithm into a linear algebra problem that can be treated numer- 
ically. In [T7] the methods developed by Kunkel and Merhmann based on the con- 
struction of normal forms were applied to predictor systems and as a particular 
instance, to singular LQ optimal problems More recently [lj have extended these 
ideas incorporating to the analysis LQ systems with index 3. Campbell's differential 
arrays method can also be applied to singular optimal control problems providing 
both a description of consistent initial conditions and the differential equation giving 
the solutions to the problem [2] . A full account of the analysis of linear differential- 
algebraic equations with variable coefficients is presented that could be applied to 
general singular LQ systems in [IS]. We have chosen however to implement the PCA 
algorithm to solve singular LQ optimal control systems because, apart that it can 
lead to computational improvements with respect to the general procedures above, it 
preserves the structure of the system. The linear DAE's that are obtained from the 
analysis of singular LQ systems carry a presymplectic structure (inherited from the 
canonical symplectic structure on the space of states and coestates). Such structure 
emerges at the end on the set of consistent states and coestates inducing there a 
(pre)symplectic structure. The reduced equations are hamiltonian with respect to 
such structure. The algorithm that will be implemented in the present paper will 
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compute the consistent initial states and the corresponding consistent coestates pre- 
serving the structure of the problem even though we will leave the analysis of the 
reduced hamiltonian equations to a continuation of this work. 

The numerical implementation of the algorithm is carefully discussed and a family 
of examples and experiments are analyzed. We will discuss experiments with large 
matrices and lower index (2 and 3) and experiments with large index n — 1, where 
n is the dimension of the state space exhibiting all of them a remarkable stable 
behaviour of the algorithm. We must point it out however that in the present state 
of the analysis of the numerical algorithm it is not possible to prove its backwards 
stability because it contains repeated products of matrices spoiling such possibility. 
More refined versions of it will be analyzed elsewhere, with a different handling of 
rank conditions that will improve its efficiency and that will allow for a rigorous error 
analysis. 

The paper will be organized as follows. First we will set up in Section [2] the basic 
notions for singular optimal problems and the particular instance of LQ systems that 
we are going to discuss. Section [3] will be devoted to review the PCA algorithm 
from the slightly wider perspective of quasilinear implicit differential equations and 
we will discuss the relation of the recursive index of the algorithm to the standard 
index. In Section [4] we will describe the linear algebraic algorithm corresponding to 
this problem and in Section [5] we will describe its numerical stability properties by 
means of various experiments. 

2. Constraint algorithms for singular LQ systems 

As it was stated in the introduction we will concentrate on the study of LQ 
optimal control systems. We will discuss the problem of finding C 1 -piecewise smooth 
curves j(t) = (x(t),u(t)) satisfying the linear control equation: 

x i = A)x j + B i a u a , (1) 

and minimizing the objective functional: 

,S( 7 )= / L(x(t),u(t))dt, (2) 

J t 

where the quadratic Lagrangian L has the form: 

L{x, u) = -Qij x i x j + N ia x i u a + -R ab u a u b , (3) 

subjected to fixed endpoints conditions: x(to) = xq, x(T) = xt (however we must 
point it out that the chosen endpoints conditions are not going to be relevant for the 
analysis to follow, hence they can be replaced by more general ones without altering 
the results presented in this paper). 

The coordinates x 1 , i = 1, . . . , n, describe points x £ IR n in state space and u a , a = 
1, . . . , m, are control coordinates defined on the linear control space MJ 11 . The matrices 
A, B, Q, N and R will be considered to be constant for simplicity. Again we must 
stress that the algorithm that we are going to discuss can be applied without difficulty 
to the time-dependent situtation, however the numerical implementation becomes 
much more involved, so we have chosen to discuss it just for time-independent systems 
in order to gain clarity and a better understanding of the experiments and their 
stability properties. 
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It is well known that normal etxtremals to this problem are provided by Pontrya- 
gin's Maximum Principle (23] : 

The curve j(t) = (x(t), u(t)) is a normal extremal trajectory if there exists a 
lifting (x(t),p(t)) of x(t) to the costate space M n xlR n satisfying Hamilton's equations: 

dpi ' l dx l ' 

where H is Pontryagin's Hamiltonian function: 

H(x,p,u) = pi (A) x j + B^u -) - L(x,u), (5) 

and the set of conditions: 

</>W(x, p, u ):= — = Pi B i a - N ia x l - R ab u b = 0. (6) 

We will call conditions in Eq. ([6]) primary constraints. Thus, trajectories solution 
to the optimal control problem must lie in the linear submanifold: 

Mi = {{x,p,u) 6 M 1 4>^l{x, P ,u) = 0}, (7) 

where M$ = { (x,p, u) G R m } denotes the total space of the system. 

If (x(t),p(t),u(t)) is a solution of the optimal problem, then its derivative must 
satisfy: 

x { = —( x ,p,u) = A)xi+Blu a , (8) 

BIT 

Pi = -T—(x,p, u) = -PjA\ + Q ijX j + N ia u a , (9) 

if = C a {x,p,u), (10) 

together with cjy ' a (x,p,u) = 0, this is, 

d4> {l ldH d<p {1 ldH | d^l cb = Q 

dx l dpi dpi dx % du b 
A simple computation shows us that if the system is regular, that is, if the matrix: 

Rab _ d 2 * 
du a du b 
is invertible in Mi, then there exists an optimal feedback condition solving Equation 
(§, 

u h = {R- l f h { Vi B i a -N ia x i ). (11) 

Then we obtain for Eq. ( |10[ ): 

u b = (R~ 1 ) ab (p l B i a -N m x l ) = 

= (R- 1 ) ab ([- Pj 4 + Qnxi + N id u d )Bl - N ia {A) x* + B\ u d ) 

Notice that in this case <jr a vanishes automatically on M±. 

However, for singular optimal LQ systems, this is, when R ab is not an invertible 
matrix, it may occur that at some points {xq,pq, uq) satisfying the primary constraint 
Eq. (16J) , that solutions of Q starting at them will not be contained in M\ for t > for 
any u. Because Eqs. Q-Q must be satisfied along optimal paths, we must consider 
only as initial conditions only those points for which there is at least a solution of 
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Q contained in M\ starting from them. Such subset is defined by the following 
conditions: 

There exists C such that <ft% := <j) {1 \ = 0, (12) 

where the derivative is taken in the direction of Eq. Q and u = C. The subset 
obtained, that we will denote by M2, is again a linear submanifold of M\ (this is 
also true in the time-dependent case) and we shall denote the functions defining M<i 
in Mi by a - We will call them secondary constraints. Notice that, in the case of 
more general endpoint conditions, that would involve considering end-time conditions 



on the coestate variables pi, the same condition Eq. (12) would apply by changing 
now the derivative of pi by the derivative along Eq. (HI) obtained by time reversing 

i4 -t. 

Clearly the argument goes on and we will obtain in this form a family of linear 
submanifolds defined recursively as follows: 

M k+1 = {(x,p,u) EM k \ 3C such that (f> {k+1) a := <p {k) a (x,p,u) = 0}. (13) 

Eventually the recursion will stop and M r = M r+ \ = M r+ 2 = • ■ ■ , for certain finite r. 
We will call the number of steps r of the algorithm before it stabilizes the recursive 
index of the problem. In this way we obtain an invariant linear submanifold, 



Moo = p| M fe , (14) 



fc>0 

that will be called the final constraint submanifold of the problem and by construction 
it consists on the set of consistent initial condition for the DAE Eqs. Q-Q. The 
geometrical analysis of this algorithm shows that this number r does not depends 
on the coordinate system and constitutes a intrinsic property of the system. Notice 
also that for general singular systems, this index may vary in principle from point 
to point. However this is not the case for singular linear systems as we will see in 
next section where we will discuss for completeness the relation between the recursive 
index r and the index of linear DAEs. 

As it was stressed in the introduction, this algorithm constitutes both an adapted 
version of the reduction algorithm for DAEs [244 [2"5] and the Presymplectic Constraint 
Algorithm (PCA). The DAE system above however has an additional structure be- 
cause it is a presymplectic system. The PCA algorithm not only determines the 
consistent initial conditions for the corresponding DAE, but in addition it provides 
the explicit form of the reduced Hamiltonian equations by computing the so called 
Dirac brackets of the system. Such brackets are obtained directly from the sequence 

(k) 

of fc-ary constraints (jr a - In this sense this algorithm is structured and preserves the 
main structure of the system along its steps. We will just proceed to the numerical 
implementation of this algorithm in its present form leaving the construction (and 
integration) of the reduced dynamical system to subsequent articles. 



3. The Kronecker and recursive index for linear DAEs 

As it was indicated before we will show for completeness the relation of the re- 
cursive index with the strangeness and differential indexes of the general theory of 
DAE's. Because of the simplicity of the problem at hand it will suffice to compute 
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the Kroneker index of the matrix pencil defining the system. Thus, we consider an 
(autonomous) quasilinear differential-algebraic equation (DAE) of the form 

A(x)x = B(x), (15) 

x £ MJ l = Mq, where A(x), B(x): M. n — > F and F is an auxiliary linear space (see 
|12j and the references therein for a geometrical treatment of DAEs). Because the 
DAE above has no additional structures the PCA becomes particularly simply and 
completely equivalent to the differential reduction by Rabier and Reinholt [?]. Even 
more, it can be written in the extremely simple form that follows. If A(x) is regular, 
we can solve explicitly x and the DAE becomes an ordinary differential equation. If 
A(x) is not regular for some x £ Mq we have to impose the constraints algorithm. 
We shall define M\ as the set of points in Mq such that B{x) £ Im.A{x). Hence 
if x £ Mi then V// £ ker A(x)* C F* it must be satisfied (/i,B(x)) = 0, where A* 
denotes the adjoint application of A. In general, 

M k+l = {xeM k \ B k (x)eImA k {x)}, (16) 

where B k = B\ M ; A k = A\ M . We obtain again 

M k+1 = {xe M k \ (n,B k (x)) = 0, V/x £ ker A k (x)*}. (17) 



In the particular case of constant linear systems, Equation (15) becomes 



A-x = B-x, (18) 



where x, x € M"; A, B £ IR n n , and the submanifold M\ defined by (16) is given 
by 

Mi = {x £lR n | B-x£lmA}, 

but this is equivalent to say that for all z verifying z T ■ A = then z T ■ B ■ x = 0. 
If z a , a = 1, . . . , m, is a basis of ker (A ), we can construct the family of primary 
constraints as 

^l(x) = z T a -B-x. 

If we define the matrix C^ l > := [zi\ •■■\z m ], then the family of linear constraints 
{(f) a}™=i is equivalent to the following matrix equation 

C^ T ■ B ■ x = 0. (19) 

Now the condition x £ Mi is equivalent to x £ ker (C^ 1 > T • B). Denoting by A k as the 
restriction of A to M k , we obtain that the linear manifold M k+ i is defined recursively 
as the set of points x £ M k such that 

c (k+i)T . B . x = 0j (20) 

where the columns of C' + ) generates the kernel of the matrix A k . 

Let A ■ x = B ■ x be a constant implicit differential-algebraic system. We will 
recall that the system is regular in the Kronecker sense if the matrix pencil A A — B, 
A £ C, is regular, i.e., if the set of solutions of the characteristic equation p(\) = 
det(AX -B) = 0is finite. 

Moreover if the pencil is regular in the Kronecker sense, then there exists regular 
matrices E, F such that |10|: 



EAF 



' I 








N 



EBF 



W 



/ 



(21) 
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where A is a nilpotent matrix with index v. In such case we will say that the index 



of the implicit differential- algebraic system given by Equation (18) is v (see [18] for a 
thourough discussion of the subject). The relation of the recursive index to the index 
of the DAE is given by the following result: 

Theorem 1. The index v of the regular constant implicit differential- algebraic system 
Ax = Bx coincides with the number of steps of the recursive constraint algorithm 
minus one: 

number of steps = r = u + 1. (22) 



Proof. If Equation (18) is a regular system in the sense of Kronecker, there exist 
regular matrices E and F such that the system becomes 

2/1 = Wyi, (23) 

Ny 2 = 2/2, (24) 

where A is nilpotent with index v (i.e. N u ^ 0, N v+l = 0) and x = Fy, together 

2/1 

2/2 

Now we only need to consider the nilpotent part of the system above, Equa- 



with y 



tion (24). Applying the recursive constraint algorithm to it, we obtain the primary 
constraint submanifold M\, given by the primary constraints 

0« = cWy 2 , 

where the columns of C^ l > generates the kernel of A. 

Moreover, Equation ( p4[ ) implies that 2/2 G M\ if and only if exists z E -Mo such 
that 2/2 = Nz, i.e., 2/2 £ ImJV. If 2/2 (t) is a curve solution of the equation, it will 
mean that y 2 (t) = Nz(t), hence fait) = Nz(t) and then 7)2 £ ImiV. So we have that 
2/2 = Nij2 = NNz = N 2 z. Then 1/2 G Mi if and only if 1/2 G ImAf 2 , and this will 
happen if and only if 

^ = C® T y 2 = 0, 
where C' 2 ** generates the kernel of A^ 2 , thus 

M 2 = {2/2 G Mi| C (2)T 2/2 = 0} , Lin{Col(C( 2 ))} = ker A 2 . 

If we proceed recursively, we can observe that 2/2 G Mfc +1 if and only if y^ £ Im A fc 
and this will take place if and only if <p > = G^'yi = 0, where the columns of C 
generates the kernel of the matrix A . As the matrix A is nilpotent with index v we 
have that 

ker A C ker A 2 C ker A 3 C . . . C ker N v C ker N v+1 = R m . 

So, in each step, the matrix C^ k > contains the previous one, C^ k ~ l \ as a submatrix, 

c (k) = [ C (fc-i)| ,,]_ 

Notice that in the i/-th step we obtain that 

y 2 eimr^) /2 =rz 1 

so, the next step is 2/2 = Aj/2 = NN U z = and the final constraint submanifold is 
M u+ \ = Mqo = {2/2 = 0}, given by the constraints 2/2 = 0. □ 
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4. A NUMERICAL LINEAR ALGEBRA ALGORITHM FOR SINGULAR LQ SYSTEMS 

Now we will adapt the general recursive constraint algorithm stated in Section [2] 
to the particular instance of singular LQ control systems. Notice that the description 
of the constrainsts algorithm done in Section [2] does not involves the use of the 
presymplectic structure, thus it is a plain constraints algorithm for a DAE. We will 
follow this approach here instead of using the full PCA algorithm because here we 
are just addressing the problem of determining the set of consistent initial conditions 
for our problem. The idea that we are going to follow to implement the algorithm is 
to transform at each step the implicit problem we have into a semi-explicit system. 
Then, we will not only get the set of constraints defining the set of consistent initial 
condition but we will have at the same time the set of explicit reduced equations of 
the system. In this form the algorithm will not provide the Hamiltonian structure of 
the equations though as it was pointed it out before. 

We will discuss now the basic idea of the algorithm from the matrix analysis 
perspective. We will write down all coordinates as column vectors: (x,p) G R n x W 1 
and u G R m . The control equation and the lagrangian density have the form already 
described in Section [2j now written in matrix notation reads as 

x = Ax + Bu, (25) 

L = -x T Qx + x T Nu + -u T Ru, (26) 

with A, Q G R nxn , B, N G R nxm and R G R mxm . The Pontryagin's Hamiltonian 
becomes: 

H(x, p, u) = p T Ax + p T Bu x T Qx - x T Nu u T Ru. (27) 



x = -^-y = Ax + Bu, (28) 



Using these notations the equations of motion (I8j)-(10) become: 

dH 
dp 

BIT 

P = -— T = -A T p + Qx + Nu, (29) 

u = C, (30) 

and the column primary constraint vector is given by: 

cj)^) ■- -N T x + B T p - Ru. (31) 

We must notice that when applying the recursive constraint algorithm to the 
problem above, that all the constraints thus obtained will be linear. Then we can 
writet them at each step k of the algorithm as: 

^ (x, p,u):= a {k) x + P^p + p {k) u, (32) 

with matrices a^ k \f3^ G R rkXn , p^ G R r ' kXm , for some r k G N. Notice that: 

a W = -N T , (3^=B T , pU = -R. 

The matrix equations 

^)=0;...;#)=0, 

define the linear manifolds M^ obtained by applying the recursive constraint algo- 
rithm. Thus the matrices cr' ', /3' ', // ' completely characterize the constraints 
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We will store these matrices in a block structured matrix 3> whose k-th row, $(£:, :), 
will be given by [ a^ | (3^ | pW> ] . 

If R is a regular matrix, then there exists an optimal feedback and the control 
variables are uniquely determined. However, if R is singular we must apply the recur- 
sive constraint algorithm. As we have discussed above, the first step of the algorithm 
amounts to study the stability of the primary constraint (j)^ 1 ', i.e., to determine for 
which points (x,p,u) there exists a vector C £ M. m satisfying 

^ =a^x + p m p + p m C = 0, (33) 

with C = it. Because p' 1 ' is singular, the linear system obtained from it 

// 'C = b(x,p,u), 

with b{x,p,u) = —a^ l 'x — P^'p, will not always have solution. However, given 
the dependence of (x,p,u) on the inhomogeneous part of the linear equation, we 
can determine for which values of them the system will have solution. For those 
points (x,p,u) such that a solution exists, i.e., such that there exists C £ M. m with 
(f)^ l '{x,p, u) = 0, we will obtain a partial optimal feedback. The remaining equations 
will impose further conditions on the points (x,p,u) where we can expect to find 
solutions to the original optimal control problem. That part will constitute what we 
were calling before the secondary constraints of the problem. 

We will obtain this separation between the partial optimal feedback and the sec- 
ondary constraints by using the singular value decomposition (SVD) of p' 1 '. There 
exists two unique orthogonal matrices, U^ 1 ', V^- 1 ', and real numbers 



.(i) 



> s 



(i) 



> 



> s ri > 0, the singular values of p^ 1 ', such that: 



p (i) = C/ (i) S (i)y(i)T = [/ (i) 



,(i) 



'n 



y(!)- 



Redefining the variables itW = V^ T u, then u« = V^ T u = V^ T C = CW, and 



[/(^(i) = uWT ^ a (D ± + £(1)^ + E (1) C (1) 



E« 



where E^ 1 ' 



ri 



diag(4 , •• 



,W> 



We will split C« as [C ( ri > , c£L ri ] T , where Q\ 



C« = 0, 



(34) 



,(i) 



are the components 1, . . . , n, of C^- 1 ' and C m _ ri the r±, . . . , m, ones. We then get a 
partial feedback for C^ and the new constraint (j)( 2 >: 

[ I ri \ ] U^ T (*^± + fiMp) + EWCW = (35) 



and, 



Im-r^U^^X + ^p 



(36) 



*-m—r\ 



UW- 



[0| 

a (2) x + l3 {Z >p + p 



(2), 



,(2) 



ii 



a WA + /3 ( VQ)x + (-/3^A T )p + (a^B + ^N)u 
0. 
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Iterating the process, given the k— th constraint <jr- ' defined in (32), the SVD of the 
matrix // ' provides orthogonal matrices LA \ V^ \ and the new constraint: 



with, 



a 



(fc+i) 



^(fc+i) 
p(*+i) 



[o|/ m _ r jc/( fe ) T (a( fc U + 

[0|/ m _ r jC/( fc ) r (-/3W^), 
[0|I ro _ Pfc ]l7<*> T (</*)£ + 0( fc >JV 



(37) 
(38) 
(39) 



The recursive relations above can be solved explicitly. First we shall denote as U k := 
[ | I m —rk ] U^ ' for each k, and then consider the simplified recurrence relations: 



a {k) A + (3^Q (40) 

-(3^A T (41) 

a^B + P^N (42) 

Before solving this set of conditions, let us compute the relation of a, f3 and p with 
a, (3 and p respectively. First we will compute such relation for (3. Notice that: 

(2) 



£(k+l) 

p(fc+l) 



0(1) = 0(1), 0( 2 ) = U 1 (-/?« A T ) = \J X W 
then for k > 2: 

0W = ^ r_0(fe-i)A T ) = 

= u k ~ x ■ ■ ■ U 2 U X (-^)A T ) = u k ~ l ■ ■ ■ uWpW. 

A similar computation shows that: 

a (k+l) =U k... U 2 U lj(k+l) j fe > L 

Finally, when computing p( k+l > we obtain the same result. 

p (k+l) =U k... JJ2JJ1 ^{k) B + p{k) N ^ =U k... C7 2 [/ l^ fc+ l)_ 



(43) 



(44) 
(45) 
(46) 



Now expanding Eqs. (40)-(42) we obtain the explicit expression for the matrices 

(47) 



£(*) 



0(1) = B T , 0( fc+1 ) = (-l) k B T (A T ) k , k > 1. 
For the matrices <j( fc ) we get: 

rfc-i 



CT 



(1) 



rT 



AT, a 



(fc+l) 



iV T A fc + 5 1 



^(-lft./l^QA*- 1 - 



i=0 



(48) 



and, finally for the matrices jcr ) we obtain: 

= -R, (t 2) - 



p(i) 

p<k+l) 



"fc-2 



N T B + B T iV, 

\fc-l D T/ A T\k-\ 



-N 1 A k - l B + (-l)*" 1 ^ (A 1 ) K - L N + 



+£ 



r 



J2(-mA T TQA 



k-2-i 



j=0 



5, fc > 2. 



(49) 



We summarize the previous findings in the following theorem: 
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Theorem 2. The constraints <jr- ' = a^> x + /3' ' p + p' ' u, of the autonomous LQ 
singular optimal control problem defined by the matrices A, P, S IR nxn ; B, Q, G 
R nxm ; R£R mxm , 



x 
L 



Ax + Bu 

-x Px + x Qu + -u Ru, 



(50) 
(51) 



are given by the following formuli: 



/3« 



B 



T 



a 



(l) 



iW 



-l) k U k ~ l 



-N 1 , p^> = -R 
U 1 B T {A T ) k - 1 



( 2 ) = U 1 (-N T B + B T N) (52) 



a 



(*) 



[/ 



fc-i 



...[/i 



,(*) 



£/ 



fe-i 



+ 5 



T 



[7 1 



fc-3 



-JV^* -1 + b t 



T T A k-2 



P 

k>2, 

k-2 

Y,(-mA T yQA 

i=0 
k-2 D T/ A T\k-2 



k-2-i 



N 1 A K ~ Z B + {-If-'B 1 {A 1 f~ A N + 



^2(-iy(A T YQA 



k—3—i 



,i=0 



B 



/c> 3, 



p {k) = j7(ik) E (fc)y(fc)T 



[7 A 



o|/ m - r(fe) ]£/ (fc)T 



(53) 
,k>2, (54) 

(55) 

(56) 

(57) 
(58) 



If we perturb the matrices A, B, Q, N,R into A + SA, B + 5B, Q + 5Q, N + 5N and 
R+5R respectively, then the matrices (3^ k \ a^ k \ p^ will be changed into /?( fc ) -f §0 ( fc ) , 
<r' ' + 5cr( k >, p^ k > + 5p^ k > . Then by using the explicit expressions Eqs. (52)-(58) it 
is a straigthforward but tedious computation to obtain the following estimates for 
the conditioning of the numerical problem of computing the matrices /?' \ o~( \ p^ > 
(k>2): 



\\0 k) \\ 
p*)|| 
\\8aW\\ 
||3F(fc)|| 

ll^ll 
llnWll 



B 



^ \\SA\\ \\5B 

\\bA\\ \\6B\\ 



\A\ 



B\ 



~ TlQlT 



C 



„ ^ IMII ll^ll /, ^ 11*011 \\ 5N \\ 



B 



IIQII 



(59) 
(60) 
(61) 



for some (small) constant C. Notice that for k > 1, the output matrices are not 
sensitive to variations on the input matrix R, which on the other hand is responsible 
for the launching of the algorithm. Thus after the first step, the singular matrix 
singular R dissapears from the computations and does not influence anymore the 
rest of the construction. 

However, in spite of the closed expressions obtained above for the constraints of 
the system, in order to construct the numerical algorithm to compute them, we will 
not use Eqs. (52)-(58) but rather on we will rely on the recursion Eqs. (37)-(39). 
The algorithm will halt whenever at the step k: 

• p^ k > is regular, then we can obtain an optimal feedback u = u(x,p) and we 
will substitute it in the equations, or, 
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• <p K > is a linear combination of the previous constraints. In such a case, there 
will exists a "gauge" freedom, i.e., some of the controls will be not be deter- 
mined. 

Thus, the scheme of the algorithm will be: 

Recursive Constraint Algorithm for singular LQ problems 



input A, B, Q, N, R, tol 

Build the constraints matrix: <I> = [a^ 1 ' /3' 1 -* /Z 1 )] 

while rank(/9, tol) is deficient & rank($, tol) increases 

(U,p,V) = SVD(p); Compute the singular value decomposition of p( k > 
Compute the iterated matrices a^ k+1 \ fj( k + 1 ) ; p( fe + 1 ) 

$ 

Build the new constraints matrix: $ 



Eliminate the dependent rows of $ 
end while 
output <J>, k 



a 



(fc+l) 0(k+i) p (fe+l) 



Note that the rank is computed as a numerical rank with tolerance tol. 
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i: j , 



Pseudocode: "final constraint submanifold" 
for linear quadratic optimal control problems 



input A, B, Q, N, R, tol 

a < N ; (3 <— B ; p < R; Initialize the variables a, (3, p 

[l,m] <— size(p); Initialize the dimension I x m of p, /=maximum value 

<& ^— [o,f3,p\; Initialize the constraints matrix $ 

$ ■<— independent rows(<J>, tol); Eliminate the dependent rows 

p ■<— 0; Where p denotes rank(&), it must be to enter in the boucle 

ife«- 1; 

while rank(p, tol)< I & rank(<I>, to/) >p 
fc«- fc + 1; 

p <— rank(<£); Update the rank of <I> 
r ■<— rank(p, to/); Update the rank of p( k > 
[l,m] ^— size(p); Update the dimensions / x m of p' ' 
[£/, p, V] «— SVD(p); Singular value decomposition of p^ k > 
U ^U T 



Compute the iterated matrices <j( fc+1 ) , (3^ k+l \ p( fe+1 ); 



a ■ 



end while 



U(r + 1 :l, 
U(r + 1:1, 
U(r + 1 :l, 



)>E + £iV] 



cr /? p 



; Add the new constraints 



independent rows(<3?, tol); Eliminate the dependent rows 



if rank(<3?, tol) <= p 
fef-jt-l 
end if 



output ($, k) 
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Pseudocode: "independent rows" 
used in the "final constraint submanifold" 



Procedure independent rows(<3?, tol) 

[l,c] ■<— size (<!?); Initialize the dimension / x c of $ 



if I > Ik rank($, tol) > then 
F<-$(1,:) 
for i = 2 : I 

F 



if rank(i ? , tol) < rank 
F i 



F 

<&(v) 



, . . , , tol } then 
; If the row i is linearly independent we will add it 



end if 
end for 
else if 

F 4— [ ]; If the matrix has zero rank or it is void, returns the void matrix 
end if 

output <I> 



5. Examples and numerical experiments 

We will discuss here some numerical experiments showing that the numerical al- 
gorithm discussed above behave as expected with respect to stability and consistency. 
The microprocessor used for the numerical computations was Pentium(R), CPU 1.60 
GHz, 3.99 MHz, 0.99 GB RAM, and the program used was MATLAB 7.0.0. 

We will describe two types of experiments concerning small (k = 3) and large 
recursive index respectively. We are constructing a class of problems that is gen- 
eral enough for the purposes of the numerical stability experiments we are going to 
describe and that we can solve and describe the solution explicitly. 

In the small index problem k = 3, we show that the algorithm is stable with 
respect to the tolerance used to compute the numerical rank, tol, and with respect 
to perturbations 5 of the data. We will also discuss the dependence with the size, n, 
of the matrices. 

For the large index ones, we analyze a problem of index n — 1, where the algorithm 
behaves properly with respect to the number of steps, both regarding the tolerance, 
tol, and the perturbation of the data, 5. 



Small index problems. Small matrices 

Consider the positive semidefinite symmetric n x n matrix R of rank 1, thus there 
will exists an orthogonal matrix U such that 

R = U T R'U (62) 
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such that all elements of R' vanish except R' u > 0. State and control spaces are 
both M n , and the total space (x,p,u) is R n . The matrix A is generic and B is an 
orthonormal matrix, B T B = I n . Finally the objective functional is constructed by 
using a generic symmetric matrix Q, a matrix N of the form N = BV where V is 
any symmetric matrix and the matrix R described above. 
The primary constraints matrix is given by 

$« = [ -N T | B T | -R ] = [ -I n | I n \-R], 
corresponding to primary constraints 

cJ)W _ -N T x + B T p -Ru = 0, 
Applying the recursive constraint algorithm we obtain: 

0(!) = (-N T A + B T Q)x - B T A T p - {B T N - N T B)u - RC, 
but the SVD of R is given by Eq. (62 ), hence the new control coordinate u^ is given 



by u = U u" 1 ' and the matrix U 1 is just the (n-l)xn matrix [0 | J n _i]?7. Hence 
multiplying $•' on the left by U 1 and taking into account that B N — N B = 0, 
we obtain the set of secondary constraints: 

(j) {2) (x, p, u) = U l {-N T A + B T Q)x - U l B T A T p = 0. 

Now, computing again the derivative of 4> we obtain the equations: 

0( 3 ) = U l {-N T A 2 +B T QA-B T A T Q)x+U 1 B T {A T fp+U 1 {B T QB-N T AB-B T A T N)i 

We observe that the matrix B T QB — N T AB — B T A T N will be invertible for generic 
A, Q, B and V. For instance if A = I, then p( 3 ' reduces to: 

p( 3 ) = B T QB - IV. 

The algorithm will stop here if det(B QB — 2V) ^ 0, this is if 2 is not an eigenvalue 
of V 1 B T QB. 

The numerical experiment of this problem will consist in applying the algorithm 
to a collection of matrices built up as a random perturbation of the matrices A, B, 
Q, V of size 5, 

A = A + 5A, \\5A\\ <5,... 
It is computed for n = 2, . . . , 202. We analyze the number of steps before the algo- 
rithm stabilizes and compute the angle, a, between the final constraint submanifold 
of the perturbed problem and the exact one, this is the error introduced in the prob- 
lem by the perturbation SA, etc. Notice that the original matrix R does not affect 
higher order constraints, hence its numerical influence restricts to launch the algo- 
rithm. Perturbations of R will not affect the computation of higher order constraints 
until it will be of the order of tol, then the algorithm will stop at the first iteration 
because if then the system will be considered to be regular. 

Table 1: First experiment (small index, small matrices). 
tol = 1(T 6 

n 5 # exact steps # steps codim a/ error 



2 


le-016 


3 


3 


4 


0.0000000000000014 


2 


le-015 


3 


3 


4 


0.0000000000000011 


2 


le-014 


3 


3 


4 


0.0000000000000059 
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Table 1: First experiment (small index, small matrices). 
tol = 1(T 6 



n 


5 


# exact steps 


# steps 


codim 


a/ error 


2 


le-013 


3 


3 


4 


0.0000000000000857 


2 


le-012 


3 


3 


4 


0.0000000000010174 


2 


le-011 


3 


3 


4 


0.0000000000035392 


2 


le-010 


3 


3 


4 


0.0000000000213074 


2 


le-009 


3 


3 


4 


0.0000000002366236 


2 


le-008 


3 


3 


4 


0.0000000095514555 


2 


le-007 


3 


3 


4 


0.0000000659481913 


2 


le-006 


3 


3 


4 


0.0000003955805449 


2 


le-005 


3 


1 


2 


0.0000048787745951 


2 


le-004 


3 


1 


2 


0.0000180580284314 


2 


le-003 


3 


1 


2 


0.0003437294288526 


2 


le-002 


3 


1 


2 


0.0052691557462037 


2 


le-001 


3 


1 


2 


0.0558945145125515 


n 


5 


j^ exact steps 


# steps 


codim 


a/ error 


102 


le-016 


3 


3 


304 


0.0000000000000071 


102 


le-015 


3 


3 


304 


0.0000000000000085 


102 


le-014 


3 


3 


304 


0.0000000000000608 


102 


le-013 


3 


3 


304 


0.0000000000005422 


102 


le-012 


3 


3 


304 


0.0000000000060781 


102 


le-011 


3 


3 


304 


0.0000000000579329 


102 


le-010 


3 


3 


304 


0.0000000005536516 


102 


le-009 


3 


3 


304 


0.0000000060825389 


102 


le-008 


3 


3 


304 


0.0000000579413997 


102 


le-007 


3 


3 


302 


0.0000004014308009 


102 


le-006 


3 


3 


146 


0.0000032681204284 


102 


le-005 


3 


3 


108 


0.0000322420378538 


102 


le-004 


3 


1 


102 


0.0003320360185045 


102 


le-003 


3 


1 


102 


0.0031921082398989 


102 


le-002 


3 


1 


102 


0.0340272669549200 


102 


le-001 


3 


1 


102 


0.1777792824454151 


n 


5 


# exact steps 


# steps 


codim 


a/ error 


202 


le-016 


3 


3 


604 


0.0000000000000078 


202 


le-015 


3 


3 


604 


0.0000000000000106 


202 


le-014 


3 


3 


604 


0.0000000000000809 


202 


le-013 


3 


3 


604 


0.0000000000007860 


202 


le-012 


3 


3 


604 


0.0000000000078201 


202 


le-011 


3 


3 


604 


0.0000000000801147 


202 


le-010 


3 


3 


604 


0.0000000008647247 


202 


le-009 


3 


3 


604 


0.0000000081096494 


202 


le-008 


3 


3 


602 


0.0000000577669246 


202 


le-007 


3 


3 


602 


0.0000005902470063 
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Table 1: First experiment (small index, small matrices). 
tol = 1(T 6 
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n 


S 


# 


exact 


steps 


# steps 


codim 


a/ error 


202 


le-006 




3 




3 


262 


0.0000048558677195 


202 


le-005 




3 




3 


208 


0.0000490952628668 


202 


le-004 




3 




1 


202 


0.0004788619712676 


202 


le-003 




3 




1 


202 


0.0046902949060197 


202 


le-002 




3 




1 


202 


0.0408456670809488 


202 


le-001 




3 




1 


202 


0.2283913263712342 



In a 




Figure 1. Error for the first experiment (small index, small matri- 
ces), n = 2, 52, 102, 152, 202. Tolerance used for the computations 
of the numerical rank equal to 10 -6 



The results show up that the algorithm works well until perturbations of order of 
5 = 10~ 6 . The least squares approximation of ln(a) versus ln(<5) gives a line of slope 
0.95, which is consistent with a = 0(5). 

In Table [T] we see that the codimension of the subspace, codim = 3n — 1, that is, 
the number of rows of the constraints matrix fails at n = 2 when 5 is of the order of 
tol. However, when n grows codim fails for smaller 5. 

Moreover the results show that the algorithm is insensitive to the size of the 
original matrices. In fact, if we select a fixed value of the perturbation, 5 = 10~ 6 , 
and analyze the error for different values of n, we obtain Table [2} 



IN 
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Table 2. First experiment (small index, small matrices). 5 = 10 9 , 
tol = 1(T 6 



# exact steps # steps codim 



a/error 



2 


le-009 


3 


3 


4 


22 


le-009 


3 


3 


64 


42 


le-009 


3 


3 


124 


62 


le-009 


3 


3 


184 


82 


le-009 


3 


3 


244 


102 


le-009 


3 


3 


304 


122 


le-009 


3 


3 


364 


142 


le-009 


3 


3 


424 


162 


le-009 


3 


3 


484 


182 


le-009 


3 


3 


544 


202 


le-009 


3 


3 


604 



0.0000000009585285 
0.0000000028931919 
0.0000000034146838 
0.0000000046018755 
0.0000000053815907 
0.0000000055767568 
0.0000000064274692 
0.0000000070707452 
0.0000000072378230 
0.0000000077680372 
0.0000000083969815 



Again data obtained indicates heuristically that a = 0(yfn 
approximation of ln(a) versus ln(n) gives us an slope of 0.47. 



The least squares 



In a 




Inn 

Figure 2. Error for the first experiment (small index, small matri- 
ces), n = 2 -> 202, 5 = 10~ 6 , tol = 10~ 6 
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Small index problems. Large matrices 

Let us consider the linear-quadratic problem where A is proportional to the iden- 
tity matrix, A = al, where q£I. 

The primary constraint will be 

0(1) = a (l) x + 0(l) p + p (l) u = _jv t x + B T p - Ru, (63) 

the general form of the (k + 1) — th constraint will be 

(fc+D = a (^) x + f3 (k+l) p + p (k + l) u=[0lIm ^J U (k)T^k) i + ^k)^ = 

= [0 I I m ^ rk ]U^ T UaaW + P {k) Q)x + {-a^)p + (a^B + (3^N)u . 

Here we use the same notation as in Section El that is, after applying the SVD to 
the matrix a^ k \ we obtain p W = [/WsWf' 7 , and U k := [ | I m _ n }U^ T . 
Computing the constraints, we obtain 

0( 2 ) = a {2) x + p^p + p^u = C/i [(aa (1) + /3 (1) Q)x + {-a^ x) )p + (a W B + pV>N)u 

= Ui [(-aN T + B T Q)x + (-aB T )p + {-N T B + B T N)u] , 

0(3) = a ^) x + ^) p + p ^) u = u 2 Uaa^ +^Q)x + {-ap { - 2) )p+{a {2) B + ^N)u = 

= U 2 Ui [(-a 2 N T + aB T Q - aB T Q)x + (a 2 B T )p + (-aN T B + B T QB - aB T N)u] 
= U 2 Ui [{-a 2 N T )x + (a 2 B T )p + (-a(N T B + £ T iV) + £ T Q£)u] , 
0(4) = ^(4)^ + £(4) p + p (4) u = ^ r ^ ao .(3) + ^(sjgjj. + (_ a 0(s)) p + ((jP's + /3( 3 )a> = 

= U z U 2 Ui [{-a 3 N T + a 2 B T Q)x + {-a 3 B T )p + (-a 2 N T B + a 2 £ T A0u] . 
So the constraints matrix will look as 



* 



Ui [-aN T + B T Q 
-U 2 Ui [a 2 N T ] 



U 3 U 2 Uia 2 \-aN T + B T Q] 



B T 

-Ui [aB T ] 

U2U1 [a 2 B T ] 

-U s U 2 Uia 2 [aB T ] 



-R 

Ui [-N T B + B T N] 
U 2 U! [a(-N T B - B T N) + B T QB] 



U 3 U 2 Uia 2 \-N T B + B T N] 



We can see that the fourth row is related with the second one by row4 = UsU 2 a 2 iow 2 , 
so the algorithm will stop here if it did not do it before. 

For the numerical implementation we choose the following matrices: Q = A = 
l n e R nxn , B T = (1,...,1) e R nxl , N T = (0,...,0) e M nxl , R = 0; so the 

constraints matrix will have only three rows 

"0,...,0 1,.. 

$= 1,...,1 -1,.., 

_0,...,0 1,.. 

where n is the dimension of the matrices A and Q. Thus in the third row we ob- 
tain optimal feedback and the final constraint submanifold is given by the following 
equations: x\ + • • • + x n = p\ + • • • + p n = u = 0. 

We apply the numerical algorithm for the previows matrices for n = 1000, tol- 
erance equal to 10~ 16 and we compare the solution obtained with the perturbed 
matrices: A = A + SA, \\SA\\ < 5, N = N + 5N, \\5N\\ < 6 and B = B + 5B, 



1 


" 


1 





1 


n 
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Table 3. Second experiment (small index, large matrices), tol = 10~ 16 

n 5 # exact steps # steps codim a/ error 

0.00000000000002 
0.00000000000005 
0.00000000000010 
0.00000000000088 
0.00000000000921 
0.00000000008945 
0.00000000094362 
0.00000000895734 
0.00000009224712 
0.00000089770415 
0.00000934812796 
0.00008953156206 
0.00088056539460 
0.00953194630784 
0.10128762315235 
0.80931668976548 

\\SB\\ < 5, where 5 = 10~ 16 — > 10 _1 . Again, as the final constraint submanifold of 
the original problem and the perturbed one must be the same, we measure the angle 
between them, this is going to be the error, and we show it in Table [3J 



Again the data shows that a = 0(5) and, consistently with the previous results, 
the slope of the least squares approximation of ln(a) versus ln(<5) is 0.96. 



1000 


le-016 


3 


3 


3 


1000 


le-015 


3 


3 


3 


1000 


le-014 


3 


3 


3 


1000 


le-013 


3 


3 


3 


1000 


le-012 


3 


3 


3 


1000 


le-011 


3 


3 


3 


1000 


le-010 


3 


3 


3 


1000 


le-009 


3 


3 


3 


1000 


le-008 


3 


3 


3 


1000 


le-007 


3 


3 


3 


1000 


le-006 


3 


3 


3 


1000 


le-005 


3 


3 


3 


1000 


le-004 


3 


3 


3 


1000 


le-003 


3 


3 


3 


1000 


le-002 


3 


3 


3 


1000 


le-001 


3 


3 


3 



A NUMERICAL ALGORITHM FOR SINGULAR OPTIMAL LQ CONTROL SYSTEMS 21 

5r 



In a 




Figure 3. Error for the second experiment (small index, large ma- 
trices), n = 1000. tol = 10" 16 



Large index problems 



Consider the following problem: 



Ac 



Q = A + A T , Be 



nnxl 



N = B,R = 0. 



(64) 



Computing the matrices /r ' , p^ ', . . . , p^ ' , we get 



(1) n( 2 ) 



#) 



? (1) 
,(2) 
,(3) 

-,( 4 ) 



i? = 0, 

£ T iV - JV T S = B T B - B T B = 0, 



-N T AB - B T A T N + B T QB = B T [-A - A T + A + A T ].B = 0, 
B T [-A T + (-A T ) 2 + (A + vl r )A - A T (A + ^ T )]5 = 0, 



fc-2 



,(*+!) 



B Tj_ A fe-i + (_i)*-i(^T)fc-i + J2(-iy(A T ) l (A + A T )A k - 2 - i ]B 



i=0 



\k-lf AT\k-l 



= b t [-A*- 1 + (-l)*- 1 ^ 1 

fc-1 fc-2 

+ J^ -(-ly^^A*- 1 -^ + ^2(-l) j {A T ) j A k - 1 - j ]B = 

3=1 j=0 

= B T [-A k ~ 1 + (-l) k ~ 1 (A T ) k - 1 - {-l) k - 1 {A T ) k - 1 + A k - X ]B = 0. 
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We obtain that these matrices are always zero and there will not exist optimal feed- 
back. Let us consider now the remaining matrices 

<t« = -B T , 

a m = _b t A + B t {A + A t ) = B t A t , 

a (3) = B T [ _ A 2 + ( A + A T )A _ A T( A + A T )] = _ B T {A T ) 2 j 



a 



(fc+l) 



fc-l 



B T [-A k + J2(-iy(A T y(A + A T )A k - l - l \ = 

i=0 

fe-i fe-i 

B T [-A k + ^(-l)*^)^^*- 1 -* + ^(-l)*^)^*-*] 



i=0 

k 



1=0 

fc-l 



B T [-^ fe + ^ -(-l) J '(^ T ) J A fc - J ' + ^(-l) J '(A T ) J 'A fe -^] 

j=l j=0 

B T [-A k - (-l) k (A T ) k + A k = {-l) k - 1 B T {A T ) k . 



/3« = B T , 



B T A T , 



0. 



£(*+!) = (-\) k B T {A T ) k . 

Hence, we will obtain 

/3 fc +! = -a k+1 = (-l) k B T (A T ) k , p k+ 

and the constraints matrix will look as follows 

-B T B T 

B T A T _ B T A T 

-B T {A T ) 2 B T {A T ) 2 



$ 







(65) 



-(-l)*B r (A T )* (-l) fe £? T (A T ) fe 

Moreover, it is clear from the previous considerations that the algorithm will stop 
only when at a given step we will obtain a linear combination of the previous rows. 
Suppose that the minimum polynomial of the matrix A is of degree q, then there are 
two possibilities for the algorithm to stop: 

• B ^ ker(A ), I = 1, . . . , q, then the row q + 1 is a linear combination of the 
previous ones. 

• B G ker(A p ), < p < q, then the row p + 1 vanish. 

We apply the algorithm to a problem where the pair (A, B) is such that A 6 
]R nx ™ is a nilpotent matrix of index n, i.e., A 11 ^ 1 ^ 0, A n = 0, and B ^ ker(A'), 



I 



1, 



,n 



1, i.e., B J =(!,...,!)€ 



)lxn 



The index of the algorithm is k 



n. 



Perturbing the matrices as: A == A + 6 A, \\5A\\ < 5, Q = A + A T , B = B + 5B, 
\\5B\\ < 5 , N = B and R = 8R, \\5Rtt < 5; with 5 = 1(T 16 -»■ 1(T 5 , we get for n = 20 
with tolerance equal to 10 -6 Table |4j 
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Table 4. Third experiment (large index), tol = 10 -6 



n 


5 


# exact steps 


# steps 


co-dim 


a/ error 


20 


le-016 


20 


20 


20 


0.00000000000001 


20 


le-015 


20 


20 


20 


0.00000000000002 


20 


le-014 


20 


20 


20 


0.00000000000001 


20 


le-013 


20 


20 


20 


0.00000000000004 


20 


le-012 


20 


20 


20 


0.00000000000021 


20 


le-011 


20 


20 


20 


0.00000000000652 


20 


le-010 


20 


20 


20 


0.00000000001940 


20 


le-009 


20 


20 


20 


0.00000000034574 


20 


le-008 


20 


20 


20 


0.00000000647005 


20 


le-007 


20 


20 


20 


0.00000006884601 


20 


le-006 


20 


20 


20 


0.00000047329442 


20 


le-005 


20 


1 


1 


0.00000157131430 



In this experiment, the value of the slope of the least squares approximation of 
the data is 0.84. 
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In a 




Figure 4. Error for the third experiment (large index), n = 20, tol = 10 
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